/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file gps.cxx
	\brief class implemenation for gps

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/


#include "sonde.h"
#include "utilities.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern bool verbose;
extern bool logging;
extern String logDir;
/*<-End Global Variables-->*/

Sonde::Sonde(String name, int a_port) : ThreadedPipe(name)
{
	sonde.id = a_port;
	sonde.gps.utc.hours = 0;
	sonde.gps.utc.minutes = 0;
	sonde.gps.utc.seconds = 0;
	sonde.gps.pos.lat = 0;
	sonde.gps.pos.lon = 0;
	sonde.gps.pos.alt = 0;
	sonde.gps.sog = 0;
	sonde.gps.track = 0;
	sonde.gps.accuracy.sats = 0;
	sonde.gps.accuracy.quality = 0;
	sonde.gps.accuracy.pdop = 0;
	sonde.pth.temp = 0;
	sonde.pth.internal_temp = 0;
	sonde.pth.rh1 = 0;
	sonde.pth.rh2 = 0;

	transmit_data = 0;
}

bool Sonde::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);

	Client * me = shmPtr->clients.getClient(0);
	shmPtr->clients.subscribe(me, CMD_GPS_TELEM);
	shmPtr->clients.subscribe(me, CMD_PTH);

	/*--------- log ----------*/
	if(logging) {
		log_name = generteNumberedFile( logDir, "pth_sensor_");
	}

	return true;
}

void Sonde::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);	                    		// zero read fd set
	FD_ZERO(&writeFDs);	                    		// zero write fd set
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo

	maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("Sonde::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			if(n>0) {
				// process read commands
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size )
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;

					return;
				}

				Command cmd((uint8_t *)fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);

					uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

					switch(cmd.cmd() ) {
						case CMD_PTH: {
							memcpy(&(sonde.pth),dataPtr,sizeof(PTH_t));
							transmit_data |= 0x1;
							} break;
						case CMD_GPS_TELEM: {
							memcpy(&(sonde.gps),dataPtr,sizeof(GPSTelem_t));
							transmit_data |= 0x2;
							} break;
					}
				}
			}
		}
		if(transmit_data == 0x3) {
			if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
				CMD_CMD((fifo_msg.data)) = CMD_SONDE;
				CMD_NUM((fifo_msg.data)) = sizeof(Sonde_t);
				memcpy( CMD_DATAPTR(fifo_msg.data), &(sonde), sizeof(Sonde_t));
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				Client * a_client;
				int i=0;
				while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_SONDE,i)) != NULL)
				{
					Client * me = shmPtr->clients.getClient(0);
					fifo_msg.src.s_addr = shmPtr->clients.getAddr(me, a_client).s_addr;
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
					writeFifo(INBOARD_TX);
					i++;
				}
				transmit_data = 0;
			}
			if(logging) {
				logfile=fopen(log_name.c_str(),"a");
				if(logfile != NULL) {
					fprintf(logfile,"(%f,%f,%f) ",sonde.gps.pos.lat,sonde.gps.pos.lon,sonde.gps.pos.alt);
					update_gps_str();
					fprintf(logfile,"%s\n", gpsStr);
					fprintf(logfile,"%f,%f,%f,%f,%f\n",sonde.pth.pressure,sonde.pth.temp,sonde.pth.internal_temp,sonde.pth.rh1,sonde.pth.rh2);
					fflush(logfile);
					fclose(logfile);
				}
			}
		}
	}
}
